/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "SonarSet.h"
#include "World.h"

CSonarSet::CSonarSet(CWorld *World, CObject *ParentObject, float interval, 
  float range, float beamAngle, MaCI::MaCICtrl::CMaCICtrlServer *mcs, int minor, 
  std::string interfaceName)
:CObject(World,ParentObject),
interval(interval),
range(range),
beamAngle(beamAngle),
timer(0),
rangingServer(mcs,minor)
{
  rangingServer.SetDeviceLimits(0,range);
  rangingServer.SetInterfaceInstanceName(interfaceName);
}
    
void CSonarSet::Activate()
{
  rangingServer.Open();
  
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
      (*i)->Activate();
  }
  
}
    
void CSonarSet::Draw()
{
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Draw();
  }

  for (PrimitivesIter i=Parts.begin();i!=Parts.end();i++)
  {
    (*i)->Draw();
  }
}
    
void CSonarSet::Update(float StepSize)
{
  timer+=StepSize;
  if (timer>=interval)
  {
    
    rangingServerData.Reset();

    rangingServerData.EncodeTo(&binbag);

    float t=SimWorld->GetSimulationTime();
    rangingServerData.SetTimestamp(MaCI::Common::TTimestamp((int)floorf(t),(int)((t-floorf(t))*1000000)));
    rangingServerData.SetDistanceHeader(MaCI::Ranging::TDistanceHeader(beamAngle,sonars.size()));
    
    for (std::vector<CSonar*>::iterator i=sonars.begin(); i!=sonars.end(); i++)
    {
      rangingServerData.AddDistance((*i)->GetReading());
    }
    rangingServer.SendDistanceArrayEvent(rangingServerData);
    timer=0;
  }
}
    
void CSonarSet::AddSonar(dBodyID parentBody, dReal *posOffset, dMatrix3 rotOffset, float direction)
{
  CSonar *s=new CSonar(SimWorld,this,parentBody,posOffset,rotOffset,range,beamAngle,direction);
  sonars.push_back(s);
  Children.push_back(s);
}

void CSonarSet::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{

}

